#include "uart.h"
#include <stdio.h>
using namespace std;

UART::UART():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,57600);
    _to = Timeout::simpleTimeout(100);

}

void UART::state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
  
}

void UART::attack_state_cb(const std_msgs::Float32MultiArray::ConstPtr& msg)
{
    // std::cout<<"----->test:: "<<attack_state<<std::endl;
    attack_state = int(msg->data[0]);
    track_Point_x = msg->data[1];
    track_Point_y = msg->data[2];
    track_Point_z = msg->data[3];
}

void UART::altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_altitude = msg.relative*100;
}

void UART::odometry_callback(const nav_msgs::Odometry &current_info)
{
    // current_altitude = current_info.pose.pose.position.z*100;

    current_angle.x= current_info.pose.pose.orientation.x;
    current_angle.y= current_info.pose.pose.orientation.y;
    current_angle.z= current_info.pose.pose.orientation.z;
    current_angle.w= current_info.pose.pose.orientation.w;
    curr_angle = toEulerAngle(current_angle);
}
void UART::velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vx = msg.twist.linear.x*100;
    current_vy = msg.twist.linear.y*100;
}

void UART::rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    _channel5_value = rcvalue.channels[4];
    _channel6_value = rcvalue.channels[5];
    _channel7_value = rcvalue.channels[6];
    _channel8_value = rcvalue.channels[7];
}

void UART::gps_callback(const sensor_msgs::NavSatFix &msg)
{
    int current_latitude = msg.latitude*1e7;
    int current_longitude = msg.longitude*1e7;
    // current_altitude = msg.altitude*100;

    Int2char current_latitude_union, current_longitude_union, current_altitude_union;
    Int2char track_point_latitude_union, track_point_longitude_union, track_point_altitude_union;
    current_latitude_union.Int = current_latitude;
    current_longitude_union.Int = current_longitude;
    current_altitude_union.Int = current_altitude;
    
    track_point_latitude_union.Int = (msg.latitude+(track_Point_y/110946.0))*1e7;
    track_point_longitude_union.Int = (msg.longitude+(track_Point_x/(111318.0*cos(msg.latitude*3.14/180.0))))*1e7;
    track_point_altitude_union.Int = track_Point_z*100;
    // current_yaw_union.Int = curr_angle.z*100;
    // current_vx_union.Int = current_vx;
    // current_vy_union.Int = current_vy;

    //header and payload

    
    uint8_t  temp1[38] = {0xb5,0x62,0xa1,0xb2,0xa1,0x26, 0x11,0x11,0x11,0x11};


    temp1[10] = current_latitude_union.Char[0];
    temp1[11] = current_latitude_union.Char[1];
    temp1[12] = current_latitude_union.Char[2];
    temp1[13] = current_latitude_union.Char[3];

    temp1[14] = current_longitude_union.Char[0];
    temp1[15] = current_longitude_union.Char[1];
    temp1[16] = current_longitude_union.Char[2];
    temp1[17] = current_longitude_union.Char[3];

    temp1[18] = current_altitude_union.Char[0];
    temp1[19] = current_altitude_union.Char[1];
    temp1[20] = current_altitude_union.Char[2];
    temp1[21] = current_altitude_union.Char[3];

    //target x
    temp1[22] = track_point_latitude_union.Char[0];
    temp1[23] = track_point_latitude_union.Char[1];
    temp1[24] = track_point_latitude_union.Char[2];
    temp1[25] = track_point_latitude_union.Char[3];

    //target y
    temp1[26] = track_point_longitude_union.Char[0];
    temp1[27] = track_point_longitude_union.Char[1];
    temp1[28] = track_point_longitude_union.Char[2];
    temp1[29] = track_point_longitude_union.Char[3];

    //target z
    temp1[30] = track_point_altitude_union.Char[0];
    temp1[31] = track_point_altitude_union.Char[1];
    temp1[32] = track_point_altitude_union.Char[2];
    temp1[33] = track_point_altitude_union.Char[3];

 
 
    temp1[7] = 0x01;
    temp1[8] = 0x01;
    temp1[9] = 0x02;
    
    //temp1[9] -> vehicle state
    if(attack_state==1)
    {
        temp1[34] = 0x01;
    }
    else if(attack_state==2)
    {
        temp1[34] = 0x02;
    }
    else
    {
        temp1[34] = 0x01;
    }


    uint8_t crc = 0;

    for(uint8_t i=0;i<35;i++)
    {
        crc += temp1[i];
    }
    temp1[35] = crc;


    // crc check
    temp1[36] = 0x0d;
    temp1[37] = 0x0a;

    if((ros::Time::now().toSec()-last_pub_time)>0.2)
    {
        write(temp1, 38);
        std::cout<<"publish once dd.... "<<std::endl;
        last_pub_time = ros::Time::now().toSec();
    }
    
    
}




void UART::OpenSerial()
{
    try {
        this->open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }

    if(this->isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return;
    }
}

uint8_t UART::CrcCheck(std::vector<uint8_t> buff,int len)
{
    uint8_t crc = 0;

    for(uint8_t i=0;i<len;i++)
    {
        crc += buff[i];
    }

    return crc;
}

geometry_msgs::Vector3 UART::toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;

    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}


void UART::RunMain()
{   
    
    uint8_t package[256];

    uint8_t rx_head = 0;
    uint8_t rx_wr_index = 0;
    uint8_t b_rx_over = 0;
    serial_process_t _serial_process;
    OpenSerial();
    ros::Rate loop_rate(100);

    ros::NodeHandle nh2;
    //advertise是接收串口数据，发送到ros
    //subscribe是接收ros数据，发送到串口
    ros::Publisher _global_pub2 = nh2.advertise<sensor_msgs::NavSatFix>("/shouchi/yindao",10);
    // ros::Publisher _global_pub3 = nh2.advertise<sensor_msgs::NavSatFix>("/fromotherfollower/global",10);
    ros::Subscriber _global_traj_sub2 = nh2.subscribe("/mavros/global_position/global",10,&UART::gps_callback,this);
    ros::Subscriber _altitude_sub2 = nh2.subscribe("/mavros/altitude",10,&UART::altitude_callback,this);
    ros::Subscriber _local_pos_sub2 = nh2.subscribe("/mavros/local_position/odom",10,&UART::odometry_callback,this);
    ros::Subscriber _local_vel_sub2 = nh2.subscribe("/mavros/local_position/velocity_local",10,&UART::velocity_callback,this);
    ros::Subscriber _rcin_sub2 = nh2.subscribe("/mavros/rc/in",10,&UART::rcin_callback,this);
    ros::Subscriber _state_sub = nh2.subscribe("/mavros/state", 10, &UART::state_cb ,this);
    ros::Subscriber _attack_state_sub = nh2.subscribe("/zhou/attack_state", 10, &UART::attack_state_cb ,this);
    last_pub_time = ros::Time::now().toSec();
    

    while(ros::ok())
    {
        //read serial
        size_t n = this->available();

        uint8_t buffer[256];
        n = this->read(buffer,n);//must! n<=256
        uint8_t b[256];
        for(uint8_t i=0;i<n;i++)
        {
            b[i] = buffer[i];
        }

        for(uint8_t i=0;i<n;i++)
        {
            // std::cout<<"b[i]: "<<+b[i]<<std::endl;
            if(b[i]==0xb5)
            {
                // std::cout<<"1111111"<<std::endl;
                rx_head = 1;
                package[rx_wr_index++] = b[i];
            }
            else if(b[i]==0x62)
            {
                // std::cout<<"2222222"<<std::endl;
                if(rx_head)
                {
                    rx_wr_index = 0;
                    b_rx_over = 0;
                }
                else
                {
                    package[rx_wr_index++] = b[i];
                    rx_head = 0;
                }

            }
            else
            {
                
                package[rx_wr_index++] = b[i];
                rx_head = 0;
                if(rx_wr_index>32) //>4*8 zanding
                {
                    // std::cout<<"3333333"<<std::endl;
                    if((rx_wr_index+2) == package[3] )
                    {

                        // std::cout<<"get the whole package."<<std::endl;
                        b_rx_over = 1;

                        _serial_process.rx_buffer.clear();
                        _serial_process.rx_buffer.push_back(0xb5);
                        _serial_process.rx_buffer.push_back(0x62);

                        for(uint8_t i=2; i<package[3];i++)
                        {
                            _serial_process.rx_buffer.push_back(package[i-2]);
                        }
                        _serial_process.index = package[3];
                        _serial_process.rx_over = 1;
                        memset(package,0,256);
                    }
                }
            }
            if(rx_wr_index>200)
                rx_wr_index=200;
        }

        if(_serial_process.rx_over==1)
        {
            std::vector<uint8_t> buffer;
            for(uint8_t i=0;i<_serial_process.index;i++)
            {

                buffer.push_back(_serial_process.rx_buffer[i]);
            }
            // std::cout<<(int)buffer[0]<<std::endl;
            if(buffer[0]==0xb5 && buffer[1]==0x62 && buffer[_serial_process.index-2]==0x0d && buffer[_serial_process.index-1]==0x0a)
            {
                // std::cout<<"check the  header successfully!"<<std::endl;
                //crc check
                uint8_t crc = CrcCheck(buffer,_serial_process.index-3);
                if(crc == buffer[_serial_process.index-3])
                {
                    uint8_t temp[_serial_process.index];
                    for(uint8_t i=0;i<_serial_process.index;i++)
                    {
                        temp[i] = buffer[i];
                    }
                    std::cout<<"check crc successfully!"<<std::endl;
                    memcpy(&_visionRec,temp+10,25);
                    //publish gps
                    sensor_msgs::NavSatFix gps_pos;
                    gps_pos.position_covariance[0] = temp[9];//_gpsRec.vehicleid;

                    gps_pos.position_covariance[1] = _visionRec.cam_roll/100.0;
                    gps_pos.position_covariance[2] = _visionRec.cam_pitch/100.0;
                    gps_pos.position_covariance[3] = _visionRec.cam_yaw/100.0;
                    gps_pos.position_covariance[4] = _visionRec.cam_offset_x/100.0;
                    gps_pos.position_covariance[5] = _visionRec.cam_offset_y/100.0;
                    gps_pos.position_covariance[6] = _visionRec.proximate_dist/100.0;
                    gps_pos.position_covariance[7] = _visionRec.command;
                    // std::cout<<"gps_pos.position_covariance[2]: "<<gps_pos.position_covariance[2]<<std::endl;
                    if(gps_pos.position_covariance[0]==1)
                    {
                        _global_pub2.publish(gps_pos); 
                        std::cout<<"receive message from  shouchi !   "<<std::endl;
                    }
                    

                }
            }

            _serial_process.rx_over = 0;
        }


        ros::spinOnce();
        loop_rate.sleep();
    }
}
